#include "ff_protocol_helper.h"
#include "debug_print.h"

uint8 ff_calculate_checksum(const uint8* data, uint8 size)
{
  uint8 result = 0;
  uint8 i;
  for (i = 0; i < size; i++)
    result ^= data[i];
  return result;
}

FF_PACKET_T* ff_packet_alloc(uint8 payload_size)
{
  uint16 size = (uint16)payload_size + 5;
  FF_PACKET_T* packet = (FF_PACKET_T*)mem_alloc(size);
  packet->frame_head = FF_FRAME_HEAD;
  packet->data_length = payload_size;
  return packet;
}

void ff_packet_fill_checksum(FF_PACKET_T* packet)
{
  uint8* ptr = (uint8*)packet;
  uint8 checksum = ff_calculate_checksum(ptr+1, packet->data_length + 3);
  ptr[packet->data_length + 4] = checksum;
}

/**
 * Push a byte into a packet.
 * @param packet: the packet to push
 * @param payload_offset: offset in the payload
 * @param data: data to push
 * @return new offset: offset+1
 */
uint8 ff_packet_push_byte(FF_PACKET_T* packet, uint8 payload_offset, uint8 data)
{
  ASSERT(payload_offset < packet->data_length);
  packet->payload[payload_offset++] = data;
  return payload_offset;
}

uint8 ff_packet_push_short(FF_PACKET_T* packet, uint8 payload_offset, uint16 data)
{
  ASSERT(payload_offset+2 <= packet->data_length);
  packet->payload[payload_offset++] = (uint8)data;
  packet->payload[payload_offset++] = (uint8)(data >> 8);
  return payload_offset;
}

uint8 ff_packet_push_data(FF_PACKET_T* packet, uint8 payload_offset, uint8* data, uint8 size)
{
  ASSERT(payload_offset + size <= packet->data_length);
  memcpy(&packet->payload[payload_offset], data, size);
  return payload_offset + size;
}
/**
 * Get a byte from packet.
 * @param payload_offset: offset in the payload. Advanced 1 byte after this function.
 */
uint8 ff_packet_get_byte(const FF_PACKET_T* packet, uint8* payload_offset)
{
  ASSERT(payload_offset);
  ASSERT(*payload_offset < packet->data_length);
  return packet->payload[(*payload_offset)++];
}

uint16 ff_packet_get_short(const FF_PACKET_T* packet, uint8* payload_offset)
{
  uint8 low = ff_packet_get_byte(packet, payload_offset);
  uint8 high = ff_packet_get_byte(packet, payload_offset);
  return (uint16)low | ((uint16)high << 8);
}

FF_PACKET_T* ff_create_send_data(uint8 payload_size, uint16 target_addr)
{
  FF_PACKET_T* packet = ff_packet_alloc(payload_size + 2);
  packet->cmd.code = FF_CMD_SEND_DATA;
  ff_packet_push_short(packet, 0, target_addr);
  return packet;
}

FF_PACKET_T* ff_create_cfg_query(uint8 cfg_code)
{
  FF_PACKET_T* packet = ff_packet_alloc(1);
  uint8 offset = 0;
  packet->cmd.code = FF_CMD_CFG;
  ff_packet_push_byte(packet, offset, cfg_code);
  return packet;
}
FF_PACKET_T* ff_create_cfg_query0(uint8 cfg_code)
{
  FF_PACKET_T* packet = ff_packet_alloc(1);
  uint8 offset = 0;
  packet->cmd.code = FF_CMD_CFG0;
  ff_packet_push_byte(packet, offset, cfg_code);
  return packet;
}

FF_PACKET_T* ff_create_cfg_set(uint8 cfg_code, uint16 value)
{
  FF_PACKET_T* packet = ff_packet_alloc(3);
  uint8 offset = 0;
  packet->cmd.code = FF_CMD_CFG;
  offset = ff_packet_push_byte(packet, offset, cfg_code);
  ff_packet_push_short(packet, offset, value);
  return packet;
}

